﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Forms;

using Microsoft.Research.TouchMouseSensor;

namespace IntelliTouchMix
{
    enum CircleIndex
    {
        Pn = 0, pP, Np, nN
    }
    public partial class BlobController
    {   
        Blobs[] currentFrame;   // The current blobs being analized in the current frame
        int[] blobsColList;     // array to hold the blobs that collide
        int numBlobsCurrentFrame;
        Blobs[][] blobHistory;  // [blobs being followed][frames]
        int[] lastFrameIndex;   // Indicates the position in blobHistory
        // Real Time
        ActionType[] blobActionType; // the blob is real time or one shot?setDistSpeedTimeVect
        EventType[] blobEventType; // the blob is real time or one shot?setDistSpeedTimeVect
        TouchMouseEventArgs[] blobTouchMouseEventArgs; // the event type of the blob
        Gesture [] blobGestureHandler;

        int[] BlobLastReport;   // the time transcured since the last report
        int maxBlobs;           // Max number of blobs that can be analized
        int nextSpot;           // Next free spot in blobHistory
        //static Form1 f;
        int[] timeBlobUnfollowed;
        // int MAX_TIME_UNFOLLOWED;
        // Gesture constantes

        static float vLimit = 0.3f;
        static float hLimit = 0.97f; // 0.9
        static int thumpLimit = 7; //6
        /// <summary>
        /// Constructor.
        /// </summary>
        /// <param name="n">Maximun number of blobs</param>
        /// <param name="nFrames">Maximun number of frames</param>
        public BlobController(int n, int nFrames)// : base()
        {
            blobHistory = new Blobs[n][];
            lastFrameIndex = new int[n];
            currentFrame = new Blobs[n];
            blobsColList = new int[n];
            timeBlobUnfollowed = new int[n];
            // Real time
            blobEventType = new EventType[n]; // the blob is real time or one shot?
            blobTouchMouseEventArgs = new TouchMouseEventArgs[n]; // the event type of the blob
            BlobLastReport = new int[n];
            blobActionType = new ActionType[n];
            blobGestureHandler = new Gesture[n];
            // MAX_TIME_UNFOLLOWED = 100; // 100 ms
            maxBlobs = n;
            nextSpot = 0;
            for (int i = 0; i < n; ++i)
            {
                blobGestureHandler[i] = null;
                blobActionType[i] = ActionType.START;
                BlobLastReport[i] = 0;
                blobTouchMouseEventArgs[i] = null;
                blobEventType[i] = EventType.UNKNOWN;
                freeFrame(i);
                currentFrame[i] = new Blobs();
                blobHistory[i] = new Blobs[nFrames];
                for (int j = 0; j < nFrames; ++j)
                    blobHistory[i][j] = new Blobs();
            }
        }

        public void actualize(TouchMouseSensorEventArgs e)
        {
            try
            {
                find(e);// the Blobs found in the current frame had been store in currentFrame
                follow(); // BlobHistory and currentFrame helped to determine who blob is which 
                findNewBlobsInCurrentFrame(); // currentFrame should not have blobs anymore
            }
            catch (Exception ex){MessageBox.Show("Er "+ex.Message+" in "+ex.StackTrace);}
        }
        /// <summary>
        /// find - Returns the array of blobs founded in the image: img \n
        /// currentFrame, will hold this array of blobs
        /// </summary>
        Blobs[] find(TouchMouseSensorEventArgs e)
        {
            int w = e.Status.m_dwImageWidth;
            int h = e.Status.m_dwImageHeight;
            byte[] img = e.Image;
            int i, size;
            if (img != null)
            {
                Blobs.setParams(e);
                numBlobsCurrentFrame = 0;
                size = w * h;
                for (i = 0; i < size; ++i)
                {
                    // Find a lid pixel
                    if (img[i] > Blobs.threshold)
                    {
                        try
                        {
                            currentFrame[numBlobsCurrentFrame].Clean();
                            currentFrame[numBlobsCurrentFrame].expand(i);
                            currentFrame[numBlobsCurrentFrame].normalize();
                            numBlobsCurrentFrame++; // blob stored in current frame
                        }
                        catch (Exception ex)
                        {
                            MessageBox.Show("Error - " + i + " \n" + ex.Message);
                        }
                    }
                }
            }
            return currentFrame;
        }
        void freeFrame(int i)
        {
            timeBlobUnfollowed[i] = 0;
            lastFrameIndex[i] = -1;
        }
        /// <summary>
        /// Iterates through blob history. Compares with the currentFrame and determines who is the following blob
        /// deletes from currentFrame the following blob
        /// </summary>
        void follow()
        {   // The currentFrame has been actualized
            Blobs bContinuacion; // the blob that follows
            //f.setText("");
            for (int i = 0; i < maxBlobs; ++i)
            {    // Iterates through blob history
                if (lastFrameIndex[i] > -1)
                {   // The entry has stored frames of the blob?
                    //f.setText("Blobs in frame: "+this.numBlobsCurrentFrame);
                    //f.addText(" storing in " + i);
                    //f.addText(" Pos: " + lastFrameIndex[i]);
                    ////f.addText(" last frame "+blobHistory[i][lastFrameIndex[i]].getTime());
                    bContinuacion = blobHistory[i][lastFrameIndex[i]];      // get the last frame of the current analizing blob
                    bContinuacion.isAnalized = true;
                    bContinuacion = newFollowingCollisionBlob(bContinuacion, currentFrame);  // Return the following blob
                    // lastFrameIndex[i] = -1;
                    //  return;
                    if (bContinuacion == null)
                    {
                        // es one shot ?
                        if (blobEventType[i] == EventType.UNKNOWN || blobEventType[i] == EventType.ONE_SHOT)
                        {
                            blobEventType[i] = EventType.ONE_SHOT;
                            timeBlobUnfollowed[i] += Blobs.eventLife;
                            //  if(timeBlobUnfollowed[i]>MAX_TIME_UNFOLLOWED){
                            // check the time of blob being invisible.
                            detectGesture(i);
                            // Delete blob current frame
                            freeFrame(i);
                        }
                        // es realtime?
                        else 
                        {
                            endRealTimeAnalysis(i,bContinuacion);
                        }
                    }
                    else
                    {
                        // Increment the blobs life with his previous frame life
                        bContinuacion.addTime(blobHistory[i][lastFrameIndex[i]]);
                        //f.addText(bContinuacion.ToString());
                        if (bContinuacion.getTime() > Util.oneShotTimeLimit)
                        {
                          //  try
                            {
                                if (blobEventType[i] == EventType.UNKNOWN)
                                {
                                    startRealTimeAnalysis(i, bContinuacion);
                                }
                                else if (blobEventType[i] == EventType.REAL_TIME)
                                {
                                    continueRealTimeAnalysis(i, bContinuacion);
                                }
                                else// Error
                                    MessageBox.Show("Assert Error should be real time, in function follow, Class blobController");
                            }
                          //  catch (Exception e) { MessageBox.Show("Maldita sea " + e.StackTrace); }
                        }
                        else
                        {
                            // save the blob in the frame       
                            blobEventType[i] = EventType.UNKNOWN;
                            saveBlobInFrame(i, bContinuacion);
                        }
                    }
                }
            }
        }
        void saveBlobInFrame(int i,Blobs bContinuation)
        {
            // save the blob in the frame
            lastFrameIndex[i]++;
            blobHistory[i][lastFrameIndex[i]].set(bContinuation);
        }
        void startRealTimeAnalysis(int i, Blobs b)
        {
            blobActionType[i] = ActionType.START;
            // indicate that now is real Time
            blobEventType[i] = EventType.REAL_TIME;
            blobTouchMouseEventArgs[i] = detectGesture(i);
            if (blobTouchMouseEventArgs[i] == null)
            {
                // No se detecto blob... ignoralo..eliminalo
                freeFrame(i);
                return;
            }
            BlobLastReport[i] = 0; // Reset time
            // Reuse the blob to store it
            lastFrameIndex[i] = 0;
            // Save it in the blob history
            blobHistory[i][lastFrameIndex[i]].set(b);
        }
        void continueRealTimeAnalysis(int i, Blobs b)
        {
            blobActionType[i] = ActionType.CONTINUE;
            if (BlobLastReport[i] > Util.reportTimeLimit)
            {
                // Override the last bloblastRealTimeBlob
                TouchMouseEventArgs e = updateRealTimeEvent(i);
                if (e == null) {
                    // Error Free all
                    freeFrame(i);
                    return;
                }
                if (GestureHandler != null) GestureHandler(this, e);
                if (blobGestureHandler[i] != null) blobGestureHandler[i](this, e);
                detectGesture(i);
                BlobLastReport[i] = 0; // Reset time
                lastFrameIndex[i] = 0;// Reuse the frame again
                // Save it in the blob history
                blobHistory[i][lastFrameIndex[i]].set(b);
            }
            else
            {
                // update report time
                BlobLastReport[i] += Blobs.eventLife;
                saveBlobInFrame(i, b);
            }
        }
        void endRealTimeAnalysis(int i, Blobs b)
        {
            blobActionType[i] = ActionType.STOP;
            //TouchMouseEventArgs e = updateRealTimeEvent(i);
            //if (GestureHandler != null) GestureHandler(this, e);
            //if (blobGestureHandler[i] != null) blobGestureHandler[i](this, e);
            freeFrame(i);
        }
        
        // Check if their are remaining blobs
        // this are new blobs that had appeard for first time in the frame
        // for each blob undetected
        void findNewBlobsInCurrentFrame()
        {
            for (int i = 0; i < numBlobsCurrentFrame; ++i)
            {
                if (!currentFrame[i].isAnalized)
                {
                    // Undetected blob, add them in an empty spot in blob history
                    int spot = nextEmptySpot();
                    ////f.setText("spot "+spot);
                    currentFrame[i].isAnalized = true;
                    blobHistory[spot][0].set(currentFrame[i]); // Copy the current frame
                    lastFrameIndex[spot] = 0;     // indicate that we are using the entry
                }
            }
        }
        int nextEmptySpot()
        {
            int j = 0;
            while (lastFrameIndex[nextSpot] > -1)
            {
                nextSpot++; j++;
                nextSpot &= 0x000000f;
                if (j > 20)
                    MessageBox.Show("Error Fatal");
            }
            return nextSpot;
        }
        // Returns the average Blob from Frames
        Blobs averageBlobFromFrames(int index)
        {
            Blobs currentBlob, r = new Blobs();
            int n = lastFrameIndex[index];
            for (int i = 0; i < n; i++)
            {
                currentBlob = blobHistory[index][i];
                r.add(currentBlob);
            }
            r.normalize(n);
            return r;
        }
        // divide the frames frames from index. The resulting frames will be the average frame from the division
        Point2f[] getVectorsBetweenCentroids(int index, int div)
        {
            Blobs currentBlob;
            int n;                      // last frame index
            int factor;                 // number of frames per division
            Blobs[] r = new Blobs[div]; // resulting blobs
            n = lastFrameIndex[index];
            factor = n / div;

            if (factor == 0) return null; // if number of frames per division is zero...


            // for each division
            for (int j = 0; j < div; j++)
            {
                r[j] = new Blobs();                 // Create the new blob and inilize it on zero
                for (int i = 0; i < factor; i++)    // for each frame in the division
                {
                    currentBlob = blobHistory[index][(j * factor) + i];
                    r[j].add(currentBlob);          // add to the current blob
                }
                r[j].normalize(factor);             // Normalize the current blob.
            }
            div--;
            Point2f[] vects = new Point2f[div];
            // r[] should increment in number of pixels
            for (int j = 0; j < div; j++) // for each vector
            {
                r[j + 1].getPos().normalize();
                r[j].getPos().normalize();
                vects[j] = r[j + 1].getPos().sub(r[j].getPos());
                // Normalize the vector
                vects[j].normalize();
            }
            return vects;
        }
        void cleanHistoryBlob()
        {
            for (int i = 0; i < this.maxBlobs; i++)
            {
                freeFrame(i);
            }
        }
        // Returns and removes form bFrame the blob thats mor likely to follow
        Blobs newFollowingCollisionBlob(Blobs b, Blobs[] bFrame)
        {
            Blobs bCol;
            int numCol = 0, i;
            float minDist = 9999;
            int minBlob = -1;
            // iterate through frame
            for (i = 0; i < numBlobsCurrentFrame; i++)
            {
                if (!bFrame[i].isAnalized)
                    // MessageBox.Show(bFrame[i].ToString() + b.ToString() + b.col(bFrame[i]));
                    //f.addText("Buscando " + bFrame[i].ToString() + b.ToString());
                    if (b.col(bFrame[i]))
                    {
                        //f.addText("Colision con "+bFrame[i].ToString());
                        blobsColList[numCol] = i;
                        numCol++;
                    }
            }
            // iterate through the collition list
            float d;
            for (i = 0; i < numCol; i++)
            {   // select the closest to the centroid
                d = bFrame[blobsColList[i]].dist(b);
                if (d < minDist)
                {
                    minBlob = blobsColList[i];
                    minDist = d;
                }
            }
            if (minBlob < 0) // fade out, no collition
                bCol = null;
            else
            {
                // Remove from bFrame and copy the blob to its new location
                // bCol = new Blobs(bFrame[minBlob]);
                bFrame[minBlob].isAnalized = true;
                bCol = bFrame[minBlob];
            }
            return bCol;
        }
        // Updates 
        DistSpeedTimeVect calcDistSpeedTimeVect(int i)
        {
            int fac = 2;
            Point2f start = blobHistory[i][0].getPos();
            Point2f mid = blobHistory[i][lastFrameIndex[i] / 2].getPos();
            Point2f fin = blobHistory[i][lastFrameIndex[i]].getPos();
            Point2f[] v = new Point2f[3];
            float[] vel = new float[fac];
            float[] dist = new float[fac];
            v[0] = mid.sub(start);
            v[1] = fin.sub(mid);
            v[2] = fin.sub(start);
            // calculate the velocity
            for (int j = 0; j < fac; j++)
            {
                dist[j] = (float)Math.Sqrt((double)v[j].dist2());
                vel[j] = dist[j] / blobHistory[i][lastFrameIndex[i] * (j + 1) / fac].getTime();
                //     MessageBox.Show("vel " + blobHistory[i][lastFrameIndex[i] * (j + 1) / fac].getTime() + " dis " + v[j].dist2());

                //  vel[1] = (float)Math.Sqrt((double)v[1].dist2()) / blobHistory[i][lastFrameIndex[i]].getTime();
            }
            DistSpeedTimeVect b = new DistSpeedTimeVect();
            b.time = blobHistory[i][lastFrameIndex[i]].getTime();
            b.vect = v;
            b.speed = 0;
            b.dist = 0;
            b.ev = blobEventType[i];
            b.s_action = blobActionType[i];
            // Add all velocities
            for (int j = 0; j < fac; j++)
            {
                b.dist += dist[j];
                b.speed += vel[j];
            } b.speed /= fac;
            return b;
        }

    }

}

